/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStationLCD.Line;
import edu.wpi.first.wpilibj.templates.PhoenixControls.LifterValues;

/**
 * code specific to teleoperated
 * @author robert
 */
public class PhoenixTeleop {
    PhoenixControls controls;
    PhoenixLifter lifter;
    PhoenixDrive drive;
    PhoenixArm arm;
    PhoenixMinibot minibot;
    Const constants;

    /**
     * initialize with controls, sensors and drive
     * @param controls robot controls
     * @param lifter robot lifter
     * @param drive robot drive
     */
    public PhoenixTeleop(PhoenixControls controls, PhoenixLifter lifter,
            PhoenixDrive drive, PhoenixArm arm, PhoenixMinibot minibot,
            Const constants) {
        this.controls = controls;
        this.lifter = lifter;
        this.drive = drive;
        this.arm = arm;
        this.minibot = minibot;
        this.constants = constants;
    }

    /**
     * starts teleop
     */
    public void start() {
        drive.encoderStart();
    }

    /**
     * stops teleop
     */
    public void stop() {
    }


    public void updateMinibot() {
        PhoenixControls.MinibotValues values = controls.gamepadMinibot();
        if(values.minibotDeployed) {
            minibot.deploy(true);
        } else {
            minibot.deploy(false);
        }
    }

    private boolean reverseWasPressed = false;
    private boolean isReversed = false;

    /**
     * handles input for driving robot
     */
    public void updateDrive() {
        PhoenixControls.JoystickValues values = controls.tankDrive();
        if(values.reverseTriggered && !reverseWasPressed) {
            reverseWasPressed = values.reverseTriggered;
            isReversed = !isReversed;
            controls.putLine(Line.kUser2, "Direction: Reversed");
        } else if (!values.reverseTriggered && reverseWasPressed) {
            reverseWasPressed = values.reverseTriggered;
            controls.putLine(Line.kUser2, "Direction: Forward");
        }

        drive.tankDrive(!isReversed ? values.leftValue : values.rightValue * -1,
                !isReversed ? values.rightValue : values.leftValue * -1);
    }

    /**
     * handles input for lifter and arm
     */
    public void updateLifter() {
        PhoenixControls.LifterValues lifterControls = controls.gamepadLifter();

        arm.setArmPower(lifterControls.armValue);
        arm.setClawOpen(lifterControls.clawOpen);

        // lifter value from buttons
        if(controls.gamePad.getRawButton(constants.LIFTERSHIFTBUTTON())) {
            if(controls.gamePad.getRawButton(constants.LIFTERDOWNBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kSlot);
            }
            else if(controls.gamePad.getRawButton(constants.LIFTERLOWBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kLowShift);
            }
            else if(controls.gamePad.getRawButton(constants.LIFTERMIDDLEBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kMiddleShift);
            }
            else if(controls.gamePad.getRawButton(constants.LIFTERHIGHBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kHighShift);
            }
        }
         else {
            if(controls.gamePad.getRawButton(constants.LIFTERDOWNBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kDown);
            }
            else if(controls.gamePad.getRawButton(constants.LIFTERLOWBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kLow);
            }
            else if(controls.gamePad.getRawButton(constants.LIFTERMIDDLEBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kMiddle);
            }
            else if(controls.gamePad.getRawButton(constants.LIFTERHIGHBUTTON())) {
                lifterPresetByButton(PhoenixLifter.LifterHeight.kHigh);
            }
        }
        lifterJoystickControl(lifterControls);
    }


    /**
     * controls lifter by joystick, stopping PID if applicable
     * @param lifterControls
     */
    private void lifterJoystickControl(LifterValues lifterControls) {
        lifter.setLifterPowerRaw(lifterControls.lifterValue);
    }

    /**
     * goes to lifter preset, starting PID if applicable
     * @param height preset height
     */
    private void lifterPresetByButton(PhoenixLifter.LifterHeight height) {
       /* if(!lifter.pidEnabled()) {
            lifter.startPID();
        }
        */

        lifter.setLifterPreset(height);

    }
}
